#include "camera_detectors.h"

#include "muduo/base/CountDownLatch.h"

using namespace perception::camera;

thread_local std::shared_ptr<PoseDetectorIhs> CameraDetectors::detector_;

CameraDetectors::CameraDetectors(int num, std::string model_path)
    : pool_("CameraDetectors")
    , model_path_(std::move(model_path))
    , logger_(rclcpp::get_logger("CameraDetectors"))
{
    muduo::CountDownLatch cdl(num);

    pool_.setThreadInitCallback([&]() {
        detector_ = PoseDetectorIhs::get_detector(model_path_);
        cdl.countDown();
    });

    pool_.start(num);
    cdl.wait();
    RCLCPP_INFO(logger_, "TheadPool(%d) for CameraDetectors init done", num);

    reid_ = ReID::get_detector("/home/ubuntu/workroot/etc/");
}

CameraDetectors::~CameraDetectors()
{
    pool_.stop();
}

std::unordered_map<std::string, std::vector<perception::camera::ObjectDetectInfoPtr>>
CameraDetectors::process(std::unordered_map<std::string, cv::Mat>& images)
{
    muduo::CountDownLatch cdl(images.size());

    std::unordered_map<std::string, std::vector<perception::camera::ObjectDetectInfoPtr>> retval = {
        { "camera0", {} },
        { "camera1", {} },
        { "camera2", {} },
        { "camera3", {} },
        { "camera4", {} },
        { "camera5", {} },
        { "camera6", {} },
    };

    for (auto& image : images) {
        auto input = image.second.clone();
        auto& result = retval[image.first];
        pool_.run([&, input]() {
            detector_->process(input, result);
            for (auto iter = result.begin(); iter != result.end();) {
                if ((*iter)->box.left_top.y < 380 * 0.05) {
                    iter = result.erase(iter);
                    continue;
                }

                if ((*iter)->box.left_top.x < 640 * 0.05 && (*iter)->box.Width() < 640 / 4) {
                    iter = result.erase(iter);
                    continue;
                }

                if ((*iter)->box.right_bottom.x > 640 * 0.95 && (*iter)->box.Width() < 640 / 4) {
                    iter = result.erase(iter);
                    continue;
                }

                iter++;
            }

            for (auto& obj : result) {
                cv::Mat fragment = input(cv::Rect(obj->box.left_top.x, obj->box.left_top.y, obj->box.Width(), obj->box.Height()));
                {
                    std::lock_guard<std::mutex> guard(reid_lock_);
                    reid_->process(fragment, obj->object_feature);
                    int64_t sum = 0;
                    for (auto& i : obj->object_feature) {
                        sum += (int64_t)(i * 1000000);
                    }
                    // std::cout << "sum:" << sum << std::endl;
                }
                // std::cout << "feat:" << obj->object_feature.size() << std::endl;
            }
            cdl.countDown();
        });
    }

    RCLCPP_INFO(logger_, " wait for %lu cameras", images.size());
    cdl.wait();

    static size_t counter = 0;
    RCLCPP_INFO(logger_, "CameraDetectors one batch done! %lu", counter++);

    return retval;
}
